#include <MsTimer2.h>

const byte pin1=2;
const byte pin2=3;
const byte pin3=4;
const byte pin4=5;
const byte pin5=7;
const byte pin6=6;
const byte pin7=8;
const byte pin8=9;

const byte ir_left = A6;
const byte ir_center = 11;
const byte ir_right = 12;

static volatile int stage1 = 0;
static volatile int stage2 = 0;

#define DIR_FORWARD   0
#define DIR_LEFT      1
#define DIR_RIGHT     2
#define DIR_BACK     3
static volatile int last_dir = DIR_FORWARD;

//8拍
const int table[8][8] = {
  {0x1,0x0,0x0,0x0},
  {0x1,0x1,0x0,0x0},
  {0x0,0x1,0x0,0x0},
  {0x0,0x1,0x1,0x0},
  {0x0,0x0,0x1,0x0},
  {0x0,0x0,0x1,0x1},
  {0x0,0x0,0x0,0x1},
  {0x1,0x0,0x0,0x1}
};

//4拍
//const int table[8][8] = {
//  {0x1,0x0,0x0,0x0},
//  {0x1,0x0,0x0,0x0},
//  {0x0,0x1,0x0,0x0},
//  {0x0,0x1,0x0,0x0},
//  {0x0,0x0,0x1,0x0},
//  {0x0,0x0,0x1,0x0},
//  {0x0,0x0,0x0,0x1},
//  {0x0,0x0,0x0,0x1},
//};

void setup() {
  // put your setup code here, to run once:
  pinMode(pin1,OUTPUT);
  pinMode(pin2,OUTPUT);
  pinMode(pin3,OUTPUT);
  pinMode(pin4,OUTPUT);
  pinMode(pin5,OUTPUT);
  pinMode(pin6,OUTPUT);
  pinMode(pin7,OUTPUT);
  pinMode(pin8,OUTPUT);

  pinMode(ir_left,INPUT);
  pinMode(ir_center,INPUT);
  pinMode(ir_right,INPUT);

  MsTimer2::set(2, go);
  MsTimer2::start();
}

void idelay(int t) {
  volatile int i,j = 0;
  for(i=0;i<t;i++)
    for(j=0;j<1024;j++);
}

void turn(int backtm, int roatetm) {
    last_dir = DIR_BACK;
    idelay(backtm);
    if(roatetm > 0) {
      last_dir = DIR_RIGHT;
    }
    else {
      last_dir = DIR_LEFT;
    }
    idelay(abs(roatetm));
    last_dir = DIR_FORWARD;
}

void loop() {
    int l = analogRead(ir_left) > 100 ? HIGH : LOW;
    int c = digitalRead(ir_center);
    int r = digitalRead(ir_right);

#if 1
    if ( l == HIGH && c == HIGH  && r == HIGH ) {
      if(last_dir == DIR_LEFT) {
        turn(1000,-300);
      }
      else if(last_dir == DIR_RIGHT)
        turn(1000,300);
      else
        turn(2000,700);
    }
  else if(l == HIGH && r == LOW) {
    if (last_dir == DIR_LEFT) {
      turn(1000,-300);
    }
    else {
      turn(1000,300);
      last_dir = DIR_RIGHT;
    }
  }
  else if(l == LOW && r == HIGH) {
    if (last_dir == DIR_RIGHT) {
      turn(1000,300);
    }
    else {
      turn(1000,-300);
      last_dir = DIR_LEFT;
    }
  }
  else if( c == HIGH) {
    if(last_dir == DIR_LEFT)
      turn(1000,-300);
    else if(last_dir == DIR_RIGHT)
      turn(1000,300);
    else
      turn(2000,700);
  }
  else {
    last_dir = DIR_FORWARD;
  }
#else
  if ( l == HIGH || c == HIGH  || r == HIGH ) {
    //last_dir = DIR_BACK;
    turn(1000,300);
  }
  else {
    last_dir = DIR_FORWARD;
  }
#endif  
}

int stage_inc(int s) {
  s+=1;
  if (s > 7)
    s = 0;

  return s;
}

int stage_dec(int s) {
  s-=1;
  if (s < 0 )
    s = 7;
  return s;
}

void step1(int s) {
    digitalWrite(pin1,table[s][0]);
    digitalWrite(pin2,table[s][1]);
    digitalWrite(pin3,table[s][2]);
    digitalWrite(pin4,table[s][3]);
}

void step2(int s) {
    digitalWrite(pin5,table[s][0]);
    digitalWrite(pin6,table[s][1]);
    digitalWrite(pin7,table[s][2]);
    digitalWrite(pin8,table[s][3]);
}

void go()
{
    step1(stage1);
    step2(stage2);

    if(last_dir == DIR_LEFT){
      stage1 = stage_dec(stage1);
      stage2 = stage_inc(stage2);
    }
    else if(last_dir == DIR_RIGHT){
      stage1 = stage_inc(stage1);
      stage2 = stage_dec(stage2);
    }
    else if(last_dir == DIR_BACK){
      stage1 = stage_dec(stage1);
      stage2 = stage_dec(stage2);
    }
    else {
      stage1 = stage_inc(stage1);
      stage2 = stage_inc(stage2);
    }
}
